%matplotlib inline
import matplotlib.pylab as plt
import numpy as np
from scipy import stats as st

カルマンフィルタ

カルマンフィルタは以下のような入出力を持つフィルタである．

• 入力 = 観測　・　時間的変化に関する情報
• 出力 = 最尤推定解(=それっぽい解)
• ただし入力には誤差がある

イメージとしては以下のような推定を行うフィルタである．

• 観測 :「昨日大阪は雨だった」
• 時間的変化に関する情報 :「東京の天気は大阪から一日遅れる」
• 最尤推定解 : 「今日東京は雨だろう」

カルマンフィルタは上記のような操作(=2コの情報を上手く統合しましょ)をきっちりと数学的に行う． しかし，その数式は一見して非常に複雑である． そこで，本ノートではカルマンフィルタをなるべく平易に解説することを目的とする．

1. カルマンフィルタと状態空間表現

1.1 状態空間表現

数学的ってどんな感じか？

カルマンフィルタは，状態空間表現という数学モデルの上に成り立つ．

状態空間表現は以下の2つから構成される．

1. 状態方程式=あるシステムを何かの変数(位置・速度・電圧・温度等)を用いて表すとき，各変数の時間的変化を表す．例えば，速度は一定だから時間変化ゼロでしょ等．システムを記述する変数は状態変数と呼ばれる．
2. 観測方程式=内部状態を外部から見ると(観測すると)どのように観測されるかを表す．例えば「ある物体の運動は，位置・速度・加速度・外力等いろいろな変数で表せるが，自分が速度センサしか持ってないから速度しか分からない」というような状態を記述しやすくなる．

一般的に状態空間表現は・・・

以下で与えられる． $$ \boldsymbol{x}_{k} = \boldsymbol{F}_{k-1} \boldsymbol{x}_{k-1} + \boldsymbol{u}_{k-1} \\ \boldsymbol{z}_k = \boldsymbol{H}_k \boldsymbol{x}_k $$

ここで各変数は以下を表す．

• 添字は時刻インデックス．
• $\boldsymbol{x}$は状態変数ベクトル．推定したいものが並ぶ．
• $\boldsymbol{F}$はダイナミクスモデル行列．過去の状態変数ベクトルが現在の状態変数ベクトルにどう遷移するかを表現する．
• $\boldsymbol{u}$は入力値ベクトル．
• $\boldsymbol{z}$は観測値ベクトル・物理的に観測可能なもの．
• $\boldsymbol{H}$は観測モデル行列．

例えば状態空間表現はこんなのがある．

車の移動モデルを考える．観測として，車両位置と車輪回転数がそれぞれ与えられるとする．このときシステムは以下のように記述される．

$$\begin{pmatrix} x \\ v \\ a \end{pmatrix}_k = \begin{pmatrix} 1 & \delta t & (\delta t)^2/2 \\ 0&1&\delta t \\ 0&0&0 \end{pmatrix} \begin{pmatrix} x \\ v \\ a \end{pmatrix}_{k-1} + \begin{pmatrix} F_{k-1} (\delta t)^2 /2m \\ F_{k-1} \delta t/m \\ F_{k-1}/m \end{pmatrix}\\ \begin{pmatrix} x \\ \omega \end{pmatrix}_k =\begin{pmatrix} 1&0&0 \\ 0&D&0 \end{pmatrix} \begin{pmatrix} x \\ v \\ a \end{pmatrix}_k $$

ここで，$\delta t $は時刻インデックスk-1からkの間の経過時間 $D$は単位距離あたりの車輪回転角度を表す車輪径パラメータ．$F$は外力，$m$は車両質量を表す．

等加速度運動では以下のように記述される．

$$\begin{pmatrix} x \\ v \\ a \end{pmatrix}_k = \begin{pmatrix} 1 & \delta t & (\delta t)^2/2 \\ 0&1&\delta t \\ 0&0&1 \end{pmatrix} \begin{pmatrix} x \\ v \\ a \end{pmatrix}_{k-1}$$$$\begin{pmatrix} x \\ \omega \end{pmatrix}_k =\begin{pmatrix} 1&0&0 \\ 0&D&0 \end{pmatrix}_k \begin{pmatrix} x \\ v \\ a \end{pmatrix}_k $$

また，等速運動では以下のように記述される． $$\begin{pmatrix} x \\ v \end{pmatrix}_k = \begin{pmatrix} 1 & \delta t\\ 0&1 \end{pmatrix} \begin{pmatrix} x \\ v \end{pmatrix}_{k-1}$$ $$\begin{pmatrix} x \\ \omega \end{pmatrix}_k =\begin{pmatrix} 1&0 \\ 0&D \end{pmatrix}_k \begin{pmatrix} x \\ v \end{pmatrix}_k $$

上記の他にも，対向二輪型車両のように左右で独立に車輪回転数が与えられる場合等も考えられる．このように，状態空間は車両の移動一つ取っても，モデルによって様々な表現がある．

カルマンフィルタの取り組む問題は・・・

以下のような状況である．

• 状態変数の真値は誰にも分からない．
• 与えられるのは誤差のある観測値だけ．
• あとは状態変数の時間的変化はこうなるだろうという予想がある．
• でも時間的変化の予測も完璧じゃない．誤差がある．

カルマンフィルタはこのような状況下で，最もそれらしい(=状況的に最も真値に近いと考えられるような)状態変数の値を推定する．

※「真値に近い値」というよりも「現状最もマシな値」を出力するイメージでいたほうが良い．データがゴミなら，いくらカルマンフィルタでもゴミみたいな推定値しか出さないから気をつける．

1.2 誤差がある場合の状態空間表現を考える

今までの状態空間表現は誤差の無い場合の数式である． カルマンフィルタではノイズを考慮した状態空間表現を用いる． ノイズはノイズでも正規分布にしたがうホワイトノイズを扱う．

1.2.1 ホワイトノイズと統計量

ホワイトノイズは・・・

以下のような性質を持つノイズである．

• ホワイトノイズと時間との間には相関がない．(ある時刻のノイズの値から，次の時刻のノイズの値を予測できない)
• ホワイトノイズ同士に相関がない．(あるノイズの値から，別のノイズの値を予測できない)
• ホワイトノイズの期待値はゼロ

ホワイトノイズを数学的に表現するために・・・

確率変数$u,v$関する，期待値，分散，共分散をそれぞれ以下のように定義する．

$E[u]$ : $u$の期待値
$Var[u]$ : $u$の分散
$Cov[u,v]$ : $u,v$の共分散

ちなみに期待値，分散，共分散について，以下の関係が成り立つ．

$$ E[au+bv]=aE[u]+bE[v] \ \ (a,b=const.)\\ Var[u]=E[(u-E[u])^2]\\ Cov[u,v]=E[(u-E[u])(v-E[v])]\\ Var[u]=Cov[u,u]$$

確率変数ベクトル$\boldsymbol{x}=[u,v,w]^{\mathrm{T}}$について，期待値ベクトル，共分散行列を以下のように表す．

$E[\boldsymbol{x}]= \left( \begin{array}{c} E[u] \\ E[v] \\ E[w] \end{array} \right) $ : 期待値ベクトル
$Cov[\boldsymbol{x}]= E\left[(\boldsymbol{x}-E[\boldsymbol{x}]) \ (\boldsymbol{x}-E[\boldsymbol{x}])^{\mathrm{T}}\right]= \begin{pmatrix} Var[u]&Cov[u,v]& Cov[u,w]\\ Cov[v,u]&Var[v]& Cov[v,w]\\ Cov[w,u]&Cov[w,v]& Var[w]\\ \end{pmatrix}$ : 共分散行列

確率変数ベクトル$\boldsymbol{x},\boldsymbol{y}$について，$\boldsymbol{x}$の各要素と$\boldsymbol{y}$の各要素が互いに独立であるとき，以下が成り立つ．(証明別添) $$Cov[\boldsymbol{x}+\boldsymbol{y}]=Cov[\boldsymbol{x}]+Cov[\boldsymbol{y}]$$ また，確率変数ベクトル$\boldsymbol{x}$と同一次元の正方係数行列$\boldsymbol{A}$について，以下が成り立つ．(証明別添) $$Cov[\boldsymbol{A}\boldsymbol{x}]=A\cdot Cov[\boldsymbol{x}] \cdot A^{\mathrm{T}}$$

上記の記法を用いることで

2つの独立なホワイトノイズ$u,v$の性質について，以下のように表せる．

$E[u]=0$ : ホワイトノイズの期待値はゼロ
$Cov[u,v]=0$ : ホワイトノイズ同士に相関はない

また，確率変数ベクトル$X=[u,v]^{\mathrm{T}}$を導入すると以下が成り立つ． $$Cov[X]=E\left[XX^{\mathrm{T}}\right]\\ Cov[X]=\begin{pmatrix} Var[u] &0 \\ 0&Var[v] \end{pmatrix}$$

1.2.2 状態空間表現とノイズ

ノイズを考慮した状態空間表現は

以下のように表される．

$$ \boldsymbol{x}_{k} = \boldsymbol{F}_{k-1} \boldsymbol{x}_{k-1} + \boldsymbol{u}_{k-1} + \boldsymbol{G}_{k-1} \boldsymbol{w}_{k-1} \\ \boldsymbol{z}_k = \boldsymbol{H}_k \boldsymbol{x}_k + \boldsymbol{v}_k $$

ここで，$\boldsymbol{w}$，$\boldsymbol{v}$はそれぞれホワイトノイズを要素とする確率変数ベクトルである．

車両の移動モデルの例への適用を考える

状態遷移ノイズ(外乱)として予期せぬ外力$f$が作用しており，観測にはそれぞれノイズ(観測誤差)が乗るものとする．この場合の状態空間表現は以下のようになる．

$$\begin{pmatrix} x \\ v \\ a \end{pmatrix}_k = \begin{pmatrix} 1 & \delta t & (\delta t)^2/2 \\ 0&1&\delta t \\ 0&0&0 \end{pmatrix} \begin{pmatrix} x \\ v \\ a \end{pmatrix}_{k-1} + \begin{pmatrix} F_{k-1}(\delta t)^2 /2m \\ F_{k-1} \delta t/m \\ F_{k-1}/m \end{pmatrix} + \begin{pmatrix} (\delta t)^2 /2m \\ \delta t/m \\ 1/m \end{pmatrix} f_{k-1}\\ \begin{pmatrix} x \\ \omega \end{pmatrix}_k =\begin{pmatrix} 1&0&0 \\ 0&D&0 \end{pmatrix} \begin{pmatrix} x \\ v \\ a \end{pmatrix}_k +\begin{pmatrix} \delta x \\ \delta\omega \end{pmatrix}_k$$

ノイズのある状態空間表現を定式化したけれど

実際には我々は，ある時刻のノイズ項$\ f_{k-1},[\delta x ,\delta\omega]^{\mathrm{T}}_{k} \ $それぞれの具体的な数値を直接的に知ることができない．そのため，状態変数の具体的な値も知ることができないという問題がある．

この問題に対してカルマンフィルタでは，状態空間の期待値と分散を用いて，もっともらしい状態変数ベクトルの値，すなわち最尤推定解の算出を行う．

※期待値・分散を考えると，以下のホワイトノイズの性質から状態空間表現から未知のノイズ項が消えてくれて扱いやすくなる．

• ホワイトノイズの期待値はゼロ
• ホワイトノイズの分散は誤差のモデル化によって与えられる

1.3 状態空間の期待値と分散

a)状態方程式の期待値について考えると

以下の式が与えられる． $$ E\left[\boldsymbol{x}_{k} \right]= E\left[ \boldsymbol{F}_{k-1} \boldsymbol{x}_{k-1} + \boldsymbol{u}_{k-1} + \boldsymbol{G}_{k-1} \boldsymbol{w}_{k-1} \right] $$

ここで，$E[au+bv]=aE[u]+bE[v] \ \ (a,b=const.)$(期待値演算の線形性)より

$$ E\left[\boldsymbol{x}_{k} \right]= \boldsymbol{F}_{k-1} \cdot E\left[\boldsymbol{x}_{k-1} \right] + E\left[ \boldsymbol{u}_{k-1} \right] + \boldsymbol{G}_{k-1} \cdot E\left[ \boldsymbol{w}_{k-1} \right] $$

$E\left[ \boldsymbol{w}_{k-1} \right]=0$(ホワイトノイズの期待値はゼロ)より

$$ E\left[\boldsymbol{x}_{k} \right]= \boldsymbol{F}_{k-1} \cdot E\left[\boldsymbol{x}_{k-1} \right] + E\left[ \boldsymbol{u}_{k-1} \right]$$

b)観測方程式の期待値について考えると

$$ E\left[ \boldsymbol{z}_k \right] = E\left[ \boldsymbol{H}_k \boldsymbol{x}_k + \boldsymbol{v}_k \right] \\ \iff E\left[ \boldsymbol{z}_k \right] = \boldsymbol{H}_k E\left[ \boldsymbol{x}_k \right] $$

c) 状態方程式の分散について考えると

$$ Cov\left[\boldsymbol{x}_{k} \right]= Cov\left[ \boldsymbol{F}_{k-1} \boldsymbol{x}_{k-1} + \boldsymbol{u}_{k-1} + \boldsymbol{G}_{k-1}\boldsymbol{w}_{k-1} \right] $$

$\boldsymbol{x},\boldsymbol{u},\boldsymbol{w}$がそれぞれ互いに独立であると仮定すると，共分散行列の加法性より $$Cov\left[\boldsymbol{x}_{k} \right]= Cov\left[ \boldsymbol{F}_{k-1} \boldsymbol{x}_{k-1} \right]+Cov\left[ \boldsymbol{u}_{k-1} \right] + Cov\left[\boldsymbol{G}_{k-1} \boldsymbol{w}_{k-1} \right] $$

確率変数ベクトルの定数倍と共分散行列の関係より $$Cov\left[\boldsymbol{x}_{k} \right]= \boldsymbol{F}_{k-1} Cov\left[\boldsymbol{x}_{k-1} \right] \boldsymbol{F}_{k-1}^{\mathrm{T}}+ Cov\left[ \boldsymbol{u}_{k-1} \right] + \boldsymbol{G}_{k-1} Cov\left[ \boldsymbol{w}_{k-1} \right] \boldsymbol{G}_{k-1}^{\mathrm{T}} $$

d) 観測方程式の分散

$$ Cov\left[ \boldsymbol{z}_k \right] = Cov\left[ \boldsymbol{H}_k \boldsymbol{x}_k + \boldsymbol{v}_k \right] \\ \iff Cov\left[ \boldsymbol{z}_k \right] = \boldsymbol{H}_k Cov\left[ \boldsymbol{x}_k \right] \boldsymbol{H}_k^{\mathrm{T}}+ Cov\left[\boldsymbol{v}_k \right] \ $$

ここまでをまとめる

• 状態方程式の期待値

$$ E\left[\boldsymbol{x}_{k} \right]= \boldsymbol{F}_{k-1} \cdot E\left[\boldsymbol{x}_{k-1} \right] + E\left[ \boldsymbol{u}_{k-1} \right]$$

• 観測方程式の期待値 $$E\left[ \boldsymbol{z}_k \right] = \boldsymbol{H}_k E\left[ \boldsymbol{x}_k \right] $$

• 状態方程式の分散 $$Cov\left[\boldsymbol{x}_{k} \right]= \boldsymbol{F}_{k-1} Cov\left[\boldsymbol{x}_{k-1} \right] \boldsymbol{F}_{k-1}^{\mathrm{T}}+ Cov\left[ \boldsymbol{u}_{k-1} \right] + \boldsymbol{G}_{k-1} Cov\left[ \boldsymbol{w}_{k-1} \right] \boldsymbol{G}_{k-1}^{\mathrm{T}} $$

• 観測方程式の分散 $$ Cov\left[ \boldsymbol{z}_k \right] = \boldsymbol{H}_k Cov\left[ \boldsymbol{x}_k \right] \boldsymbol{H}_k^{\mathrm{T}}+ Cov\left[\boldsymbol{v}_k \right] \ $$

見やすくするために

以下のように変数をおきなおす．

$$E\left[\boldsymbol{x} \right] \iff \boldsymbol{x} \\ E\left[\boldsymbol{u} \right] \iff \boldsymbol{u} \\ Cov\left[\boldsymbol{x} \right] \iff \boldsymbol{P} \\ Cov\left[\boldsymbol{w} \right] \iff \boldsymbol{Q} \\ Cov\left[\boldsymbol{v} \right] \iff \boldsymbol{R} \\ Cov\left[\boldsymbol{v} \right] \iff \boldsymbol{R} \\ Cov\left[\boldsymbol{z} \right] \iff \boldsymbol{Z} \\ $$

このとき，状態空間表現は以下のようになる． $$ \boldsymbol{x}_{k} = \boldsymbol{F}_{k-1} \boldsymbol{x}_{k-1} + \boldsymbol{u}_{k-1} \\ \boldsymbol{z}_{k} = \boldsymbol{H}_{k} \boldsymbol{x}_{k}\\ \boldsymbol{P}_{k} = \boldsymbol{F}_{k-1}\boldsymbol{P}_{k-1}\boldsymbol{F}_{k-1} + \boldsymbol{U}_{k-1}+ \boldsymbol{G}_{k-1}\boldsymbol{Q}_{k-1}\boldsymbol{G}_{k-1} \\ \boldsymbol{Z}_{k} = \boldsymbol{H}_{k}\boldsymbol{P}_{k}\boldsymbol{H}_{k}+ \boldsymbol{R}_{k} $$

このような状態空間表現を前提として，カルマンフィルタは観測値ベクトルから現在の状態変数ベクトルの最尤推定解を算出する．

1.4 カルマンフィルタ概要

1.4.1 カルマンフィルタの数式

Wikipediaを見ると

カルマンフィルタは大きく分けて予測更新ステップと観測更新ステップに分かれる．

予測更新ステップは以下の式で表わされる

• $ {\hat{\boldsymbol{x}}_{k|k-1}} = \boldsymbol{F}_{k-1}\hat{\boldsymbol{x}}_{k-1|k-1} + \boldsymbol{u}_{k-1} $ （予測推定値）
• $\boldsymbol{P}_{k|k-1} = \boldsymbol{F}_{k-1} \boldsymbol{P}_{k-1|k-1} \boldsymbol{F}_{k-1}^\textrm{T} + \boldsymbol{G}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{G}_{k-1}^\textrm{T} +\boldsymbol{U}_{k-1}$ （予測推定値の共分散行列）

観測更新ステップは以下の式で表わされる．

• $\boldsymbol{e}_k = {\boldsymbol{z}_k} - \boldsymbol{H}_k\hat{\boldsymbol{x}}_{k|k-1}$ （観測残差）
• $\boldsymbol{S}_k = \boldsymbol{R}_k + \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^\textrm{T}$ （観測残差の共分散）
• $\boldsymbol{K}_k = \boldsymbol{P}_{k|k-1}\boldsymbol{H}_k^\textrm{T}\boldsymbol{S}_k^{-1}$ （カルマンゲイン）
• $\hat{\boldsymbol{x}}_{k|k} = {\hat{\boldsymbol{x}}_{k|k-1}} + \boldsymbol{K}_k \boldsymbol{e}_k$ （最尤推定値）
• $\boldsymbol{P}_{k|k} = (\boldsymbol{E} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1}$ （最尤推定値の共分散）

これらの数式に出てくるベクトル・行列は

全て前節までに定義した記号と対応している．ただし，添字に関しては以下のようなルールに基づいている．

• ${\hat{\boldsymbol{x}}_{k|k}}$ : 時刻kの時点(現時点)で推定した時刻kの(現時点での)$\boldsymbol{x}$の最尤推定解(今の最尤推定解と名付ける)
• ${\hat{\boldsymbol{x}}_{k-1|k-1}}$ : 時刻k-1の時点(昔)で推定した時刻k-1の(その時点での)$\boldsymbol{x}$の最尤推定解(昔の最尤推定解と名付ける)
• ${\hat{\boldsymbol{x}}_{k|k-1}}$ : 時刻k-1の時点(昔)で推定した時刻kの(現時点の/その時点から見れば未来の)$\boldsymbol{x}$の最尤推定解(予測値と名付ける)

カルマンフィルタの流れは

以下のようになっている

1. カルマンフィルタで現時点での最尤推定解${\hat{\boldsymbol{x}}_{k|k}}$が与えられた状態から考え始める．
2. 次の時刻になりました． この時点で今の最尤推定解${\hat{\boldsymbol{x}}_{k|k}}$は現時点から見れば昔の最尤推定解${\hat{\boldsymbol{x}}_{k-1|k-1}}$になる．
3. 昔の最尤推定解とダイナミクスモデルを使って今の予測値を得る．(${\hat{\boldsymbol{x}}_{k|k-1}}= \boldsymbol{F}_{k-1}\hat{\boldsymbol{x}}_{k-1|k-1}$)
4. 今の時刻における観測値$\boldsymbol{z}_k$が得られる．
5. 予測値と観測値のずれ(観測残差)$\boldsymbol{e}_k$を使って，今の最尤推定値${\hat{\boldsymbol{x}}_{k|k}}$が得られる．(${\hat{\boldsymbol{x}}_{k|k}} = {\hat{\boldsymbol{x}}_{k|k-1}} + \boldsymbol{K}_k \boldsymbol{e}_k$)
6. 次の時刻になりました．2に戻る．

1.4.2 カルマンフィルタの難しさ

ここまでの解説で，カルマンフィルタに出てくるベクトル・行列，添字，そしてその処理の流れについての説明が全て終了した．したがって，対象とするシステムの状態空間の期待値と共分散に関する表現が立式できれば，カルマンフィルタの原理を知らなくとも上式を当てはめるだけで最尤推定が可能である．逆に言えば，カルマンフィルタの性能は100%状態空間表現の立式の妥当性，すなわちモデル化精度に依存する．真に難しいのはカルマンフィルタの数式の難解さではなく，妥当なモデルの設計にある．

ちなみに我々が設計するべき具体的なものは以下である．

• 状態空間ベクトルの初期値$\boldsymbol{x}_0$
• 状態空間ベクトルの初期共分散$\boldsymbol{P}_0$
• ダイナミクスモデル$\boldsymbol{F}$
• 観測モデル$\boldsymbol{H}$
• 状態遷移誤差$\boldsymbol{Q}$
• 観測誤差$\boldsymbol{R}$

2．単変数，単観測カルマンフィルタと重み付き平均

本章では単変数，単観測という簡単な場合のカルマンフィルタについて解説する．特に，重み付き平均という概念を導入し，以下のことを示す．

• 重み付き平均が最尤推定解を与えること．
• 単変数単観測カルマンフィルタの観測更新が重み付き平均に等しいこと

2.1 重み付き平均の導入

とあるロボットの位置を求めたい

そのロボットの位置について・・・

• センサAは「6だ」と言った
• センサBは「12だ」と言った

このとき，最も尤もらしいロボットの位置はどこか？

解答例としては

平均をとって「9」

図にすると

Xa,Xb=6,12
Xave=(Xa+Xb)/2

# plot
plt.plot(Xa,0,'ro',label='SensorA')
plt.plot(Xb,0,'o',label='SensorB')
plt.plot(Xave,0,'ok',label='Mean')
plt.xlim(0,18)
plt.xlabel('Position of Robot')
plt.legend()
plt.grid()

では，もしセンサA,Bの精度が違ったら・・・

• センサAは「6±3(1σ)だ」と言った
• センサBは「12±2(1σ)だ」と言った

このとき，最も尤もらしいロボットの位置はどこか？

図にすると

Sa,Sb=3,2 #sigma a,b 
GaussPdf = lambda mean,std : (np.linspace(mean-3*std,mean+3*std,100), st.norm.pdf(loc=mean,scale=std,x=np.linspace(mean-3*std,mean+3*std,100)))

# plot
plt.plot(Xa,0,'ro')
plt.plot(Xb,0,'o')

plt.plot(Xave,0,'ok',label='Mean')
plt.plot(GaussPdf(Xa,Sa)[0],GaussPdf(Xa,Sa)[1],'r',label='SensorA')
plt.plot(GaussPdf(Xb,Sb)[0],GaussPdf(Xb,Sb)[1],'b',label='SensorB')

plt.xlim(0,18)
plt.ylim(-0.1,0.3)
plt.xlabel('Position of Robot')
plt.ylabel('Probability Density function value')
plt.legend(loc=0)
plt.grid()

図を見るとセンサBを信頼したくなる

というのも，センサBのほうが確率密度関数が尖っているからだ．したがって，ロボットの位置は黒色で表わされる平均値点よりもセンサB側(右側)にロボットがいると考えるのが妥当そうだ．

このような「センサBを信頼したい」というニーズに応える計算が**重み付き平均**である．

重み付き平均の式は

以下の式で表される．

$$\hat{x} = \frac{ \sigma_A^2 x_B + \sigma_B^2 x_A} {\sigma_A^2 + \sigma_B^2 } $$

ここで，センサA,Bの指し示すデータをそれぞれ$x_A$，$x_B$，センサA,Bの分散$Var[x_A],Var[x_B]$をそれぞれ$\sigma_A^2$，$\sigma_B^2$とおいた． また，重み付き平均値の標準偏差は誤差の伝搬法則より以下で与えられる．(証明別添)

$$\frac{1}{\sigma_\hat{x}^2} = \frac{1}{\sigma_A^2} + \frac{1}{\sigma_B^2} $$$$\Leftrightarrow \sigma_\hat{x} =\sqrt{ \frac{\sigma_A^2 \sigma_B^2}{\sigma_A^2+\sigma_B^2}} $$

分かること

実際に計算して図示すると，単純な平均値よりもセンサBに重きをおいた値が算出されていることが分かる． また，その分散は，センサBよりも小さい(=確率密度関数が尖っている)

重み付き平均とは何者なのか？

実は，そもそも重み付き平均は確率的に最尤推定値を計算式しようとして出来上がった計算式(証明別添) つまり，重み付き平均は最尤推定の考え方から導出される式で，最尤値を計算できるということ

Xhat=(Sa**2 *Xb + Sb**2 *Xa) / (Sa**2 + Sb**2)
Shat=np.sqrt( (Sa**2 * Sb**2) / (Sa**2 + Sb**2) )

# plot
plt.plot(Xa,0,'ro')
plt.plot(Xb,0,'o')
plt.plot(Xave,0,'ok',label='Mean')
plt.plot(Xhat,0,'om')
plt.plot(GaussPdf(Xa,Sa)[0],GaussPdf(Xa,Sa)[1],'r',label='SensorA')
plt.plot(GaussPdf(Xb,Sb)[0],GaussPdf(Xb,Sb)[1],'b',label='SensorB')
plt.plot(GaussPdf(Xhat,Shat)[0],GaussPdf(Xhat,Shat)[1],'m',label='Weighted Mean')

plt.xlim(0,18)
plt.ylim(-0.1,0.3)
plt.xlabel('Position of Robot')
plt.ylabel('Probability Density function value')
plt.legend(loc=0)
plt.grid()

#print
print('Weighted Mean =', '{0:4.2f}'.format(Xhat), '(nearer to Xb(=', Xb ,') than Xave(=',Xave,')' )
print('Weighted Mean Std =','{0:4.2f}'.format(Shat), '< SensorB std =',Sb )

Weighted Mean = 10.15 (nearer to Xb(= 12 ) than Xave(= 9.0 )
Weighted Mean Std = 1.66 < SensorB std = 2

2.2.データを「修正する 」という考え方

式の構造に着目してみる

まずはカルマンフィルタの式を再掲する．

予測更新

• $ \color{red}{\hat{\boldsymbol{x}}_{k|k-1}} = \boldsymbol{F}_{k-1}\hat{\boldsymbol{x}}_{k-1|k-1} + \boldsymbol{u}_{k-1} $ （予測推定値）
• $\boldsymbol{P}_{k|k-1} = \boldsymbol{F}_{k-1} \boldsymbol{P}_{k-1|k-1} \boldsymbol{F}_{k-1}^\textrm{T} + \boldsymbol{G}_{k-1} \boldsymbol{Q}_{k-1} \boldsymbol{G}_{k-1}^\textrm{T} +\boldsymbol{U}_{k-1}$ （予測推定値の共分散行列）

観測更新

• $\boldsymbol{e}_k = {\boldsymbol{z}_k} - \boldsymbol{H}_k\hat{\boldsymbol{x}}_{k|k-1}$ （観測残差）
• $\boldsymbol{S}_k = \boldsymbol{R}_k + \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^\textrm{T}$ （観測残差の共分散）
• $\boldsymbol{K}_k = \boldsymbol{P}_{k|k-1}\boldsymbol{H}_k^\textrm{T}\boldsymbol{S}_k^{-1}$ （カルマンゲイン）
• $\hat{\boldsymbol{x}}_{k|k} = \color{red}{\hat{\boldsymbol{x}}_{k|k-1}} + \boldsymbol{K}_k \boldsymbol{e}_k$ （最尤推定値）
• $\boldsymbol{P}_{k|k} = (\boldsymbol{E} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1}$ （最尤推定値の共分散）

式の構造として，最尤推定値を算出しているのは「観測更新」の部分． ここで注目したいのは，赤字で示したように，観測更新では予測推定値がそのままの形で使われていること．つまり，予測推定値を修正する(修正量を加算する)ことで最尤推定値を得ているということ

データを修正するイメージ

を左側の図に示す．このイメージでは最尤推定値を求める式が$\hat{\boldsymbol{x}}= \boldsymbol{x}_A + \delta \boldsymbol{x} $のように，「データ」+「そのデータの修正量」の形で表される．

それに対して，重み付き平均の式のイメージを右側の図に示す．この図では二つのデータを式$f(\boldsymbol{x}_A,\boldsymbol{x}_B)$に代入することで最尤推定値を得ている．

カルマンフィルタでは，左側の図のように，データを修正するイメージで最尤推定値を算出している． 先程赤字で示した予測推定値$\hat{\boldsymbol{x}}_{k|k-1}$は図でいうところの$\boldsymbol{x}_A$に相当する．そして，予測推定値の修正量$\delta \boldsymbol{x} $を計算するために，観測値等を使っている．なお，観測値$\boldsymbol{z}_k$は図でいうところの$\boldsymbol{x}_B$に相当する．

2.3.カルマンフィルタと重み付き平均

「重み付き平均値を算出する」のではなく「重み付き平均値へと修正する」ように考えると

重み付き平均もカルマンフィルタも最尤推定値を得る手法であるから，共通点を見つけることができる．

導出は$\delta x = \hat{x} - x_A $から出発する．この式に重み付き平均値の式 $\hat{x} = \frac{ \sigma_A^2 x_B + \sigma_B^2 x_A} {\sigma_A^2 + \sigma_B^2 } $ を代入して変形していく． \begin{eqnarray} \delta x&=&\hat{x} - x_A \\ &=& \frac{ \sigma_A^2 x_B + \sigma_B^2 x_A} {\sigma_A^2 + \sigma_B^2 } - x_A \\ &=& \frac{\sigma_A^2}{\sigma_A^2+\sigma_B^2} (x_B - x_A) \end{eqnarray}

よって，カルマンフィルタのように変形した重み付き平均の式は以下で表わされる．

$$\hat{x} = x_A + \frac{\sigma_A^2}{\sigma_A^2+\sigma_B^2} (x_B - x_A) $$

カルマンフィルタと重み付き平均を再掲する

カルマンフィルタ 観測更新式

• $\boldsymbol{e}_k = {\boldsymbol{z}_k} - \boldsymbol{H}_k\hat{\boldsymbol{x}}_{k|k-1}$ （観測残差）
• $\boldsymbol{S}_k = \boldsymbol{R}_k + \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^\textrm{T}$ （観測残差の共分散）
• $\boldsymbol{K}_k = \boldsymbol{P}_{k|k-1}\boldsymbol{H}_k^\textrm{T}\boldsymbol{S}_k^{-1}$ （カルマンゲイン）
• $\iff \boldsymbol{K}_k = \boldsymbol{P}_{k|k-1}\boldsymbol{H}_k^\textrm{T}(\boldsymbol{R}_k + \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^\textrm{T})^{-1}$
• $\hat{\boldsymbol{x}}_{k|k} = {\hat{\boldsymbol{x}}_{k|k-1}} + \boldsymbol{K}_k \boldsymbol{e}_k$ （最尤推定値）
• $\boldsymbol{P}_{k|k} = (\boldsymbol{E} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1}$ （最尤推定値の共分散）

重み付き平均式

• $\hat{x} = x_A + \frac{\sigma_A^2}{\sigma_A^2+\sigma_B^2} (x_B - x_A) $ (最尤推定値)
• $\frac{1}{\sigma_\hat{x}^2} = \frac{1}{\sigma_A^2} + \frac{1}{\sigma_B^2} $ (最尤推定値の分散)
• $\iff \sigma_\hat{x}^2 =\frac{\sigma_A^2 \sigma_B^2}{\sigma_A^2+\sigma_B^2} $

いざ見比べてみると

以下のような対応を考えることで，カルマンフィルタの観測更新と重み付き平均が同じ構造になっていることが分かる． $$ \mathtt{KalmanFilter} \ \ \mathtt{VS}\ \ \mathtt{WeightedMean} \\ \hat{\boldsymbol{x}}_{k|k-1} \ \iff x_A \\ \boldsymbol{P}_{k|k-1} \ \iff \sigma_A^2 \\ \boldsymbol{z}_k \ \iff x_B \\ \boldsymbol{R}_k \ \iff \sigma_B^2 \\ \boldsymbol{H}_k \ \iff 1 \\ \boldsymbol{e}_k={\boldsymbol{z}_k} - \boldsymbol{H}_k\hat{\boldsymbol{x}}_{k|k-1} \ \iff e \equiv (x_B - x_A)\\ \boldsymbol{K} =\boldsymbol{P}_{k|k-1}\boldsymbol{H}_k^\textrm{T}(\boldsymbol{R}_k + \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^\textrm{T})^{-1} \ \iff K \equiv \sigma_A^2 (\sigma_B^2 + \sigma_A^2)^{-1} = \frac{\sigma_A^2 }{\sigma_A^2 + \sigma_B^2}\\ \hat{\boldsymbol{x}}_{k|k} = {\hat{\boldsymbol{x}}_{k|k-1}} + \boldsymbol{K}_k \boldsymbol{e}_k \ \iff \hat{x} = x_A +K e\\ \boldsymbol{P}_{k|k}=(\boldsymbol{E} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} \ \iff \sigma_\hat{x}^2= (1-\frac{\sigma_A^2 }{\sigma_A^2 + \sigma_B^2})\sigma_A^2 =\frac{\sigma_A^2 \sigma_B^2}{\sigma_A^2+\sigma_B^2} $$

なお，上記の表中にて，以下の式を定義した． $$e=(x_B - x_A) \\ K=\frac{\sigma_A^2}{\sigma_A^2+\sigma_B^2}$$

このように，重み付き平均は単変数，単観測カルマンフィルタの観測更新に等しいと見ることができる．

ここまでの観測更新を図にすると

以下のような形になる．

2.4 カルマンゲインの意味

ここで，カルマンゲイン$K$の役割について考える

最尤推定値算出の式を展開すると以下のようになる．

$$\hat{x} = x_A +K e\\ \iff \hat{x} = x_A +K(x_B-x_A)\\ \iff \hat{x} = x_A +Kx_B - Kx_A$$

a)$x_A$の信頼度がデータ$x_B$と比較して非常に高い場合

$\sigma_A << \sigma_B$を当てはめると以下のように$K=0$が示される． $$K=\frac{\sigma_A^2}{\sigma_A^2+\sigma_B^2} \\ \iff K=\frac{\sigma_A^2 /\sigma_B^2 }{\sigma_A^2/\sigma_B^2+\sigma_B^2/\sigma_B^2}\\ \iff K=\frac{0 }{0+1}=0$$

このとき，最尤推定値は以下のようになる． $$\hat{x} = x_A +Kx_B - Kx_A \\ \iff \hat{x} = x_A$$

つまり，$K=0$のとき，カルマンフィルタはデータ$x_B$を無視して，$x_A$を全面信頼する．

b) データ$x_B$の信頼度がデータ$x_A$と比較して非常に高い場合

$\sigma_A << \sigma_B$を用いてカルマンゲインを計算すると，$以下のようにK=1$が示される． $$K=\frac{\sigma_A^2}{\sigma_A^2+\sigma_B^2} \\ \iff K=\frac{\sigma_A^2 /\sigma_A^2 }{\sigma_A^2/\sigma_A^2+\sigma_B^2/\sigma_A^2}\\ \iff K=\frac{1}{1+0}=1$$

このとき，最尤推定値は以下のようになる． $$\hat{x} = x_A +Kx_B - Kx_A \\ \iff \hat{x} = x_B$$

つまり，$K=1$のとき，カルマンフィルタはデータ$x_A$を無視して，$x_B$を全面信頼する．

以上を総合すると

カルマンゲインとは2つのデータのどちらを信頼すべきかを示す重みであると言える．

更に具体的に言えば，2つのデータの分散比によって一意に決定されるパラメータである．このことを強く意識するとカルマンゲインは以下のような数式にするのが分かりやすいかもしれない． $$K=f\left(\frac{\sigma_B^2}{\sigma_A^2}\right)=\frac{1}{1+\frac{\sigma_B^2}{\sigma_A^2}}$$ 以下にそのグラフを示す．

Var = lambda r : 1/(1+r)
r = np.linspace(0,30,500)

# plot
plt.figure()
plt.plot(r,Var(r),label='K=1 at ratio=0 , K=0 at ratio=infinity')
plt.xlabel('Variance Ratio')
plt.ylabel('KalmanGain')
plt.grid()
plt.legend()

plt.figure()
plt.plot(r,Var(r),label='K=0.5 at ratio=1')
plt.xlabel('Variance Ratio')
plt.ylabel('KalmanGain')
plt.grid()
plt.xlim(0,2)
plt.legend()

<matplotlib.legend.Legend at 0x25b2ed915f8>

2.5 単変数単観測カルマンフィルタ完成+ブロック線図

ここまでの観測更新を図にすると

以下のような形になる．

ここまで$x_A$はセンサAからの出力(観測)と考えていたけれど

以下のように考えれば，別にセンサ使わなくても現在の状態量に関する情報は手に入る．

• センサA　「今の状態量はこうなっているはずだ」
• 私　　 　「前の時刻の状態量から予測すると，今の状態量はこうなっているはずだ」

この考えを数式に落とし込んでみる．

• 前の時刻の状態量 = $\hat{x}_{k-1|k-1}$
• 状態量の時間変化を予測するモデル = $\boldsymbol{F}_{k}$
• 前の時刻の状態量から「状態量の時間変化の予測」を使うと今の状態量の予測値 = $\boldsymbol{F}_{k}\hat{x}_{k-1|k-1}$
• 今の状態量の予測値に名前をつける =$\hat{x}_{k|k-1} = \boldsymbol{F}_{k}\hat{x}_{k-1|k-1}$

最後に，センサAの出力$x_A$の代わりに現在の状態量の予測値$\hat{x}_{k|k-1}$を使えばカルマンフィルタが完成する．図にすると以下のようになる．

k-1からkへの時刻変化を行列$Z^{-1}$で表現すると以下の図になる．

3. 一般のカルマンフィルタ

重み付き平均の多次元への自然な拡張として以下の式を導入すればうまく説明できそうな気がする(予想)

$$\hat{\boldsymbol{x}} = \frac{ Cov[\boldsymbol{x_A}] \boldsymbol{x_B} + Cov[\boldsymbol{x_B}] \boldsymbol{x_A}} { Cov[\boldsymbol{x_A}] + Cov[\boldsymbol{x_B}]} \\ \iff \hat{\boldsymbol{x}} = \left( Cov[\boldsymbol{x_A}] \boldsymbol{x_B} + Cov[\boldsymbol{x_B}] \boldsymbol{x_A} \right) \left( Cov[\boldsymbol{x_A}] + Cov[\boldsymbol{x_B}]\right)^{-1} $$

4. 拡張カルマンフィルタ

今までのカルマンフィルタは線形状態空間を対象とする線形カルマンフィルタだった． 線形状態空間は以下のような状態空間である． x=Fx

本章では非線形状態空間を対象とする非線形カルマンフィルタ（拡張カルマンフィルタ・EKF）を扱う． 非線形状態空間は以下のような状態である． x=f(x) 拡張カルマンフィルタの大まかな流れは，非線形状態空間を線形空間に近似して，線形カルマンフィルタを適用する．

その前に線形・非線形とは 行列形式ではなくスカラーで考える 線形 y=ax 非線形 y=f(x) 例えばf(x)=x^2とする．描画すると明らかに直線ではない=線の形をしていない=線形でない．

線形化 拡張カルマンフィルタの大まかな流れは，非線形状態空間を線形空間に近似して，線形カルマンフィルタを適用する． これを線形近似とか線形化という

ではf(x)=x^2を直線で近似するにはどうしたら良いだろうか？ 何の条件も無いとどうしたら良いか分からない． そこで，「〇〇近傍で」という便利な言葉を導入する． 例えばこんな直線はx=x近傍ではおおよそx^2とみなしても良いとする． 例えばx=x1ではy=a1x,x=x2ではy=a2xな直線になる． では，任意のx近傍における近似直線はどう表わされるか？ こんな式になる．びぶーん

この近似を使うと，任意のxからdxだけ移動した時のf(x)の変化分df(x)を記述することができる． delta f(x)=df(x)/dx * delta x dletax->inf でdf=dfていう当たり前の式になんるけど，二次元以上だと変わる 偏微分がはいる 二次元だと，曲面を線形(平面)近似することになる． delta f(x) = rf(x)/rx + rf(x)/ry

行列表現を使って一般化すると rF/rx

これが線形化 すると状態空間表現はこうなる あとはどこ近傍？というのが重要になる

Discrete-time predict and update equations[edit] Predict[edit] Predicted state estimate

$${\displaystyle {\hat {\boldsymbol {x}}}_{k|k-1}=f({\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k})} $$

Predicted covariance estimate
$${\displaystyle {\boldsymbol {P}}_{k|k-1}={{\boldsymbol {F}}_{k-1}}{\boldsymbol {P}}_{k-1|k-1}{{\boldsymbol {F}}_{k-1}^{\top }}+{\boldsymbol {Q}}_{k}} $$

Update[edit] Innovation or measurement residual

$${\displaystyle {\tilde {\boldsymbol {y}}}_{k}={\boldsymbol {z}}_{k}-h({\hat {\boldsymbol {x}}}_{k|k-1})} $$

Innovation (or residual) covariance

$${\displaystyle {\boldsymbol {S}}_{k}={{\boldsymbol {H}}_{k}}{\boldsymbol {P}}_{k|k-1}{{\boldsymbol {H}}_{k}^{\top }}+{\boldsymbol {R}}_{k}} $$

Near-optimal Kalman gain
$${\displaystyle {\boldsymbol {K}}_{k}={\boldsymbol {P}}_{k|k-1}{{\boldsymbol {H}}_{k}^{\top }}{\boldsymbol {S}}_{k}^{-1}} $$ Updated state estimate
$${\displaystyle {\hat {\boldsymbol {x}}}_{k|k}={\hat {\boldsymbol {x}}}_{k|k-1}+{\boldsymbol {K}}_{k}{\tilde {\boldsymbol {y}}}_{k}} $$

Updated covariance estimate $${\displaystyle {\boldsymbol {P}}_{k|k}=({\boldsymbol {I}}-{\boldsymbol {K}}_{k}{{\boldsymbol {H}}_{k}}){\boldsymbol {P}}_{k|k-1}}$$

where the state transition and observation matrices are defined to be the following Jacobians

$${\displaystyle {{\boldsymbol {F}}_{k-1}}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k}}} $$$${\displaystyle {{\boldsymbol {H}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}} $$

付録

A. 共分散行列の性質

A_1 共分散行列の加法性

$\boldsymbol{x},\boldsymbol{y}$を確率変数ベクトルとする．$\boldsymbol{x}$の各要素と$\boldsymbol{y}$の各要素が独立であるとき，以下の式(共分散行列の加法性)が成り立つ． $$Cov\left[\boldsymbol{x}+\boldsymbol{y}\right]=Cov\left[\boldsymbol{x}\right]+Cov\left[\boldsymbol{y}\right]$$

[証明]

任意の確率変数$x,y$について，期待値，共分散に関する以下の式が成り立つことを前提とする． $$E\left[x+y\right]=E\left[x\right]+E\left[y\right] \ \ (1)$$ $$Cov\left[x,y\right]=E\left[xy\right]-E\left[x\right]E\left[y\right] \ \ (2)$$ また，互いに独立な任意の確率変数$x,y$について，期待値に関する以下の式が成り立つことを前提とする． $$ E\left[xy\right]=E\left[x\right]E\left[y\right] \ \ (3) $$

共分散行列の任意の$\ i\ $行$\ j\ $列目の要素について考える．確率変数ベクトルが$\boldsymbol{x}=\left[x_1,x_2,\cdots,x_n\right]$で与えられるとき，確率変数ベクトル$\boldsymbol{x}$の共分散行列の任意の$\ i\ $行$\ j\ $列目の要素$Cov\left[\boldsymbol{x}\right]_{i,j}$は以下で表わされる． $$Cov\left[\boldsymbol{x}\right]_{i,j}=Cov\left[x_i,x_j\right]$$

したがって，証明したい命題の任意の$\ i\ $行$\ j\ $列目の要素を取り出すと以下のようになる． $$Cov\left[\boldsymbol{x}+\boldsymbol{y}\right]_{i,j}=Cov\left[\boldsymbol{x}\right]_{i,j}+Cov\left[\boldsymbol{y}\right]_{i,j} \\ \iff Cov\left[x_i+y_i,x_j+y_j\right] = Cov\left[x_i,x_j\right]+Cov\left[y_i,y_j\right] $$

上記の式の左辺を展開して右辺に等しくなることを示す．まず，(2)式より，以下が成り立つ． $$\begin{eqnarray} Cov\left[x_i+y_i,x_j+y_j\right]&=&E\left[(x_i+y_i)(x_j+y_j)\right]-E\left[x_i+y_i\right]E\left[x_j+y_j\right] \\ &=&E\left[x_ix_j+x_iy_j+y_ix_j+y_iy_j \right]-E\left[x_i+y_i\right]E\left[x_j+y_j\right] \end{eqnarray}\ \ (4)$$ (1)式より，$E$の中身を分解する．まず(4)式右辺第一項について以下が成り立つ．

$$\begin{eqnarray} E\left[x_ix_j+x_iy_j+y_ix_j+y_iy_j \right] =E\left[x_ix_j\right]+E\left[x_iy_j\right]+E\left[y_ix_j\right]+E\left[y_iy_j\right] \end{eqnarray}$$

ここで，$\boldsymbol{x}$の各要素と$\boldsymbol{y}$の各要素が独立であることから，式(3)を用いて(4)式右辺第一項を以下のように変形できる． $$ E\left[x_ix_j\right]+E\left[x_iy_j\right]+E\left[y_ix_j\right]+E\left[y_iy_j\right] = E[x_ix_j]+E[x_i]E[y_j]+E[y_i]E[x_j]+E[y_iy_j] $$

次に，(4)式右辺第二項について式(1)より以下が成り立つ．

$$\begin{eqnarray} -E\left[x_i+y_i\right]E\left[x_j+y_j\right] &=&- (E[x_i]+E[y_i])(E[x_j]+E[y_j])\\ &=&- \left(E[x_i]E[x_j]+E[x_i]E[y_j]+E[y_i]E[x_j]+E[y_i]E[y_j]\right) \end{eqnarray}$$

以上をまとめて(4)式右辺を計算すると以下のようになる． $$\begin{eqnarray} & &E[x_ix_j]&+E[x_i]E[y_j]&+E[y_i]E[x_j]&+E[y_iy_j]\\ &-)&E[x_i]E[x_j]&+E[x_i]E[y_j]&+E[y_i]E[x_j]&+E[y_i]E[y_j]\\ \end{eqnarray}$$

$$=E[x_ix_j]-E[x_i]E[x_j]+E[y_iy_j]-E[y_i]E[y_j]$$

さらに，式(2)を用いて変形すると以下のようになる． $$E[x_ix_j]-E[x_i]E[x_j]+E[y_iy_j]-E[y_i]E[y_j]=Cov[x_i,x_j]+Cov[y_i,y_j]$$

したがって，共分散行列の任意の$\ i\ $行$\ j\ $列目について，以下の式が成り立つ． $$Cov\left[x_i+y_i,x_j+y_j\right] = Cov\left[x_i,x_j\right]+Cov\left[y_i,y_j\right]$$

以上の議論は共分散行列の全ての行，列に関して成り立つから，確率変数ベクトルを用いた表記にすることで以下の題意が示される． $$Cov\left[\boldsymbol{x}+\boldsymbol{y}\right]=Cov\left[\boldsymbol{x}\right]+Cov\left[\boldsymbol{y}\right]$$

A_2 確率変数ベクトルの定数倍と共分散行列

確率変数ベクトル$\boldsymbol{x}$と同一次元の正方係数行列$\boldsymbol{A}$について，以下が成り立つ． $$Cov[\boldsymbol{A}\boldsymbol{x}]=A\cdot Cov[\boldsymbol{x}] \cdot A^{\mathrm{T}}$$

[証明] $$ Cov[\boldsymbol{x}]= E\left[(\boldsymbol{x}-E[\boldsymbol{x}]) \ (\boldsymbol{x}-E[\boldsymbol{x}])^{\mathrm{T}}\right] $$ より，以下が成り立つ． $$\begin{eqnarray} Cov[\boldsymbol{Ax}]&=&E\left[(\boldsymbol{Ax}-E[\boldsymbol{Ax}]) \ (\boldsymbol{Ax}-E[\boldsymbol{Ax}])^{\mathrm{T}}\right]\\ &=&E\left[(\boldsymbol{Ax}-\boldsymbol{A}\cdot E[\boldsymbol{x}]) \ (\boldsymbol{Ax}-\boldsymbol{A}\cdot E[\boldsymbol{x}])^{\mathrm{T}}\right]\\ &=&E\left[\boldsymbol{A}(\boldsymbol{x}-E[\boldsymbol{x}]) \ \left(\boldsymbol{A}(\boldsymbol{x}- E[\boldsymbol{x}])\right)^{\mathrm{T}}\right]\\ &=&E\left[\boldsymbol{A}(\boldsymbol{x}-E[\boldsymbol{x}]) \ (\boldsymbol{x}- E[\boldsymbol{x}])^{\mathrm{T}} \boldsymbol{A}^{\mathrm{T}} \right]\\ &=&\boldsymbol{A} \cdot E\left[(\boldsymbol{x}-E[\boldsymbol{x}]) \ (\boldsymbol{x}- E[\boldsymbol{x}])^{\mathrm{T}} \right]\cdot \boldsymbol{A}^{\mathrm{T}} \\ &=&\boldsymbol{A} \cdot Cov\left[\boldsymbol{x}\right]\cdot \boldsymbol{A}^{\mathrm{T}} \\ \end{eqnarray}$$

B. 最尤推定法

B_1 最尤推定法による重み付き平均導出

ある同一の値に関する誤差のある観測値$x_A$，$x_B$が与えられ，その分散$\sigma_A^2$，$\sigma_B^2$がそれぞれ正規分布に従うとき， 以下の式は最尤推定解を与える． $$\hat{x} = \frac{ \sigma_A^2 x_B + \sigma_B^2 x_A} {\sigma_A^2 + \sigma_B^2 } $$

[証明]

考え中 独立事象=積の法則=確率密度P(a)×P(b)の最大化 グラフにすると正方形面積最大化に対応するはず 何か面白い証明をしたい

Xa,Xb,Xhat=6,12,10.15
Sa,Sb=3,2 #sigma a,b 
# st.norm.pdf(loc=mean,scale=std,x=x)
'''
for Xtrue in np.linspace(6,12,10):
    plt.figure()
    xq=lambda std : np.linspace(Xtrue-3*std,Xtrue+3*std,100)
    plt.plot(xq(Sa),st.norm.pdf(loc=Xtrue,scale=Sa,x=xq(Sa)),'b')
    plt.plot(xq(Sb),st.norm.pdf(loc=Xtrue,scale=Sb,x=xq(Sb)),'r')
    
    plt.plot(Xa,st.norm.pdf(loc=Xtrue,scale=Sa,x=Xa),'ob')
    plt.plot(Xb,st.norm.pdf(loc=Xtrue,scale=Sb,x=Xb),'or')
    print(Xtrue,st.norm.pdf(loc=Xtrue,scale=Sa,x=Xa),st.norm.pdf(loc=Xtrue,scale=Sb,x=Xb),st.norm.pdf(loc=Xtrue,scale=Sa,x=Xa)*st.norm.pdf(loc=Xtrue,scale=Sb,x=Xb))
    
    plt.grid()
    plt.xlim(0,15)'''
    
plt.figure()
#Xtrue= np.linspace(6,12,100)
Xtrue= np.linspace(-10,30,500)
plt.plot(st.norm.pdf(loc=Xtrue,scale=Sa,x=Xa),st.norm.pdf(loc=Xtrue,scale=Sb,x=Xb),'k')
plt.plot(st.norm.pdf(loc=Xa,scale=Sa,x=Xa),st.norm.pdf(loc=Xa,scale=Sb,x=Xb),'or')
plt.plot(st.norm.pdf(loc=Xb,scale=Sa,x=Xa),st.norm.pdf(loc=Xb,scale=Sb,x=Xb),'ob')
plt.plot(st.norm.pdf(loc=Xhat,scale=Sa,x=Xa),st.norm.pdf(loc=Xhat,scale=Sb,x=Xb),'ob')

[<matplotlib.lines.Line2D at 0x25b2f631630>]

C. 誤差の伝播法則

誤差の伝播法則は以下のような問題に適用可能な法則である．

• 円の面積を求めたい．そこで円の半径を定規で計測したところ5cm$\pm$0.1cmだった．円の面積の精度はいくらか？
• 車両の走行距離を求めたい．車輪の一周長が30cm$\pm$0.2cm，回転数が100回転$\pm$0.3回転だった．走行距離の精度はいくらか？

これらの問題は以下のように定式化される．

• 互いに独立な確率変数$x_1,x_2,\cdots x_n$に対し，それぞれの分散が与えられている．このとき以下の値を求めよ．

$$Var\left[f(x_1,x_2,\cdots x_n)\right]$$

• 誤差の伝播法則は以下の式で与えられる．

$$Var\left[f(x_1,x_2,\cdots x_n)\right] = \left(\frac{\partial f}{\partial x_1}\right)^2 Var[x_1] + \left(\frac{\partial f}{\partial x_2}\right)^2 Var[x_2] \cdots \left(\frac{\partial f}{\partial x_n}\right)^2 Var[x_n] $$

C_1 誤差の伝播法則による重み付き平均値の分散導出

2変数の重み付き平均値を出力する関数 $$f(x_A,x_B) = \frac{ \sigma_A^2 x_B + \sigma_B^2 x_A} {\sigma_A^2 + \sigma_B^2 } $$ の分散は以下で与えられる． $$ \sigma_\hat{x}^2 ={ \frac{\sigma_A^2 \sigma_B^2}{\sigma_A^2+\sigma_B^2}} $$

[証明] 誤差の伝播法則より以下が成り立つ． $$Var[f(x_A,x_B)] = \left(\frac{\partial f}{\partial x_A}\right)^2 Var[x_A] +\left(\frac{\partial f}{\partial x_B}\right)^2 Var[x_B]$$ ここで，偏微分項は以下のように計算される． $$\left(\frac{\partial f}{\partial x_A}\right) = \frac{\sigma_B^2}{\sigma_A^2+\sigma_B^2}$$ $$\left(\frac{\partial f}{\partial x_B}\right) = \frac{\sigma_A^2}{\sigma_A^2+\sigma_B^2}$$

したがって，以下が成り立つ． $$ Var[f(x_A,x_B)] = \left( \frac{\sigma_B^2}{\sigma_A^2+\sigma_B^2} \right)^2 \sigma_A^2 + \left( \frac{\sigma_A^2}{\sigma_A^2+\sigma_B^2}\right)^2 \sigma_B^2 \\ = \frac{1}{(\sigma_A^2+\sigma_B^2)^2} (\sigma_A^2\sigma_B^4 + \sigma_B^2 \sigma_A^4) \\ = \frac{\sigma_A^2\sigma_B^2}{(\sigma_A^2+\sigma_B^2)^2} (\sigma_B^2+ \sigma_A^2) \\ = \frac{\sigma_A^2\sigma_B^2}{(\sigma_A^2+\sigma_B^2)}\\ $$

C_2 誤差の伝播法則の一般形

確率変数x_1,x_2が独立なとき，誤差の伝播法則は以下． $$Var\left[f(x_1,x_2)\right] = \left(\frac{\partial f}{\partial x_1}\right)^2 Var[x_1] + \left(\frac{\partial f}{\partial x_2}\right)^2 Var[x_2]$$

独立でないとき，噂によると以下の式になるらしい． $$Var\left[f(x_1,x_2)\right] = \left(\frac{\partial f}{\partial x_1}\right)^2 Var[x_1] + \left(\frac{\partial f}{\partial x_2}\right)^2 Var[x_2] + 2 \left(\frac{\partial f}{\partial x_1}\right) \left(\frac{\partial f}{\partial x_2}\right)Cov[x_1,x_2]$$

非常に規則的なので確率変数ベクトル$\boldsymbol{x}=[x_1,x_2,\cdots ,x_n]^{\mathrm{T}}$を用いて一般形にしてみる． まずベクトルによる偏微分を以下に定義する． $$\left(\frac{\partial f}{\partial \boldsymbol{x} }\right) = \begin{pmatrix} \frac{\partial f}{\partial x_1 } \\ \frac{\partial f}{\partial x_2 } \\ \vdots \\ \frac{\partial f}{\partial x_n } \\ \end{pmatrix}$$

誤差の伝播法則は確率変数ベクトルを用いると以下のように表わされる． $$\left(\frac{\partial f}{\partial \boldsymbol{x} }\right)^{\mathrm{T}} Cov[\boldsymbol{x} ] \left(\frac{\partial f}{\partial \boldsymbol{x} }\right) $$

これはベクトルの二次形式と呼ばれる形で楕円と関係が深い．なかなかおもしろいことが起こりそう．